/*
 * commlib.c
 *
 *  Created on: Apr 12, 2015
 *      Author: mxw
 */
#include "commlib.h"
#include <UartDrv.h>
#include <Console.h>

#include <option.h>
#include <S3C2440addr.h>

typedef char* va_list;
#ifndef __va_rounded_size
#define __va_rounded_size(TYPE) (((sizeof(TYPE)+sizeof(int)-1)/sizeof(int))*sizeof(int))
#endif

#ifndef va_start
#define va_start(AP, LASTARG)   (AP = ((char *)& (LASTARG) + __va_rounded_size(LASTARG)))
#endif

#ifndef va_arg
#define va_arg(AP, TYPE)        (AP += __va_rounded_size(TYPE), *((TYPE *)(AP - __va_rounded_size(TYPE))))
#endif

#ifndef va_end
#define va_end(AP)              (AP = (va_list)0 )
#endif

#define A_LOW_CASE (0)
#define A_UP_CASE   (1)

void *memset(void *s, int c, unsigned long n) {
    char* p = s;
    int i = 0;
    for(; i < n; ++i) {
        p[i] = (char)c;
    }
    return 0;
}

char* memcpy(char* dst, const char* src, int n) {
    int i = 0;

    for(; i < n; ++i) {
        dst[i] = src[i];
    }
    return dst;
}

int strlen(const char* str) {
    int i = 0;
    while(str[i]) {
        ++i;
    }
    return i;
}

char* strcpy(char* dst, const char* src) {
    int i = 0;
    while(src[i]) {
        dst[i] = src[i];
        ++i;
    }
    dst[i] = src[i];    //copy the src last '\0'
    return dst;
}

// len 33
//uart0rec12345678abcd1234567899912  32
char* strcpyn(char* dst, const char* src, int dst_buf_max) {
    int copy_len = strlen(src); //33
    
    if(copy_len > (dst_buf_max - 1)) { //33 >= 31 true
        copy_len = dst_buf_max - 1; //copylen = 32 - 1  = 31
        memcpy(dst, src, copy_len); //uart0rec12345678abcd12345678999
        dst[dst_buf_max - 1] = '\0';    //dst[31] = '\0'
        return dst;
    }
    strcpy(dst, src);
    return dst;
}

int strcmp(const char* s1, const char* s2) {
    int i = 0;
    while(s1[i] && s2[i]) {
        if(s1[i] != s2[i]) {
            return (int)(s1[i] - s2[i]);
        }
        ++i;
    }
    if(s1[i] == s2[i]) { // s1[i] == s2[i] == '\0'
        return 0;
    }
    return (int)(s1[i] - s2[i]);
}

char* reverse_str(char* s) {
    int len = strlen(s);
    int i = 0;
    for(; i < (len / 2); ++i) {
        char tmp_c = s[i];
        s[i] = s[len - i - 1];
        s[len - i - 1] = tmp_c;
    }
    return s;
}

unsigned int __udivsi3(unsigned int a, unsigned int b) {
    unsigned int res = 0;
    while(a >= b) {
        a -= b;
        res++;
    }
    return res;
}

unsigned int __umodsi3(unsigned int a, unsigned int b) {
    while(a >= b) {
        a -= b;
    }
    return a;
}

int i2HexStr(unsigned int v, char* out_buf, int bigX) {
    char i2a_map_up[] = "0123456789ABCDEF";
    char i2a_map[] = "0123456789abcdef";
    int rigt = 0;
    int i = 0;
    
    if(0 == v) {
        out_buf[0] = '0';
        return 0;
    }
    
    while(v) {
        rigt = (v & 0xF);
        if(bigX) {
            out_buf[i++] = i2a_map_up[rigt];
        } else {
            out_buf[i++] = i2a_map[rigt];
        }        
        v = (v >> 4);
    }
    reverse_str(out_buf);
    return 0;
}

//0xFDDD1234, 16, A_UP_CASE(1), outbuf
int itoa(int i, int base, int aux, char* buf) {
    char i2a_map[] = "0123456789abcdef";
    char i2a_map_up[] = "0123456789ABCDEF";
    int rigt = 0; //the most right pos's value
    int j = 0; //itorator
    unsigned int u = 0;
    unsigned int _u = 0;

    u = (unsigned int)i;
    if((u & 0x80000000) && ('d' == aux)) {//neg-pos
        u = (~u + 1);//get the 2's complement
        ++j;
        buf[0] = '-';
    }

    //calc out_buf_len -> j
    _u = u;
    do {
        ++j;
        _u /= base; 
    } while(_u);

    //set str-end '\0'
    _u = u;
    rigt = _u % base;
    buf[j] = '\0';

    //set string from right of the num
    do {
        if(A_UP_CASE == aux) {
            buf[j - 1] = i2a_map_up[rigt];
        } else {
            buf[j - 1] = i2a_map[rigt];
        }
        --j;
        _u /= base;
        rigt = _u % base;
    } while(_u);
    return 0;
}

int vsprintf(char* buf, const char* format, va_list va) {
    const char* itf = format;
    int st = 0;
    char itoa_buf[32] = {0}, c;
    while(*itf) {
        switch(st) {
        case 0://idle
            if('%' == *itf) {
                st = 1;
            } else {
                *buf++ = *itf;
            }
            break;
        case 1:
            if ('x' == *itf) {//catch %x
                int i = va_arg(va, int);
                memset(itoa_buf, 0, sizeof(itoa_buf));
                i2HexStr(i, itoa_buf, A_LOW_CASE);
                int l = strlen(itoa_buf);
                memcpy(buf, itoa_buf, l);
                buf += l;
                st = 0;
            } else if('X' == *itf) {//catch %X
                int i = va_arg(va, int);
                memset(itoa_buf, 0, sizeof(itoa_buf));
                i2HexStr(i, itoa_buf, A_UP_CASE);
                int l = strlen(itoa_buf);
                memcpy(buf, itoa_buf, l);
                buf += l;
                st = 0;
            } else if('c' == *itf) {//catch %c
                c = va_arg(va, int); //char is promoted to int when passed through ‘...’
                *buf++ = c;
                st = 0;
            } else if('s' == *itf) {//catch %s
                char* s = va_arg(va, char*);
                if(!s) {
                    s = "(null)";
                }

                int l = strlen(s);
                memcpy(buf, s, l);
                buf += l;
                st = 0;
            } else if('d' == *itf) {//catch %d
                int i = va_arg(va, int);
                itoa(i, 10, 'd', itoa_buf);
                int l = strlen(itoa_buf);
                memcpy(buf, itoa_buf, l);
                buf += l;
                st = 0;
            } else if('u' == *itf) {//catch %u
                int i = va_arg(va, unsigned long);
                itoa(i, 10, -1, itoa_buf);
                int l = strlen(itoa_buf);
                memcpy(buf, itoa_buf, l);
                buf += l;
                st = 0;
            } else if('%' == *itf) {
                *buf++ = '%';
                st = 0;
            } else {
                *buf++ = '%';
                *buf++ = *itf;
                char* s = "(not surport)";
                int l = strlen(s);
                memcpy(buf, s, l);
                buf += l;
                st = 0;
            }
            break;
        default:
            break;
        }
        ++itf;
    }
    return 0;
}

int printf(const char* format, ...) {
    char buf[512] = {0};
    CMsg* msg = (CMsg*)&buf[0];
    va_list va;
    va_start(va, format);
    vsprintf(msg->data, format, va);
    
    msg->msg_id = MSG_ID_UART_SEND_REQ;
    msg->data_len = strlen(msg->data);
    gConSend.sendMsg(&gConSend, msg);
    //uart_print(buf);
    return 1;
}

int printfk(const char* format, ...) {//for svc, output immediatly
    char buf[512] = {0};
    va_list va;
    va_start(va, format);
    vsprintf(buf, format, va);
    uart_printk(buf);
    return 1;
}

int sprintf(char* buf, const char* format, ...) {
    va_list va;
    va_start(va, format);
    return vsprintf(buf, format, va);
}

//WARNING: This function is only for none-interupt such as privilege mode, 
//         because this function worked like blocked
void delay_ms(int time_ms) {
	unsigned int val = (PCLK >> 3) / 1000 - 1;
	
	rTCFG0 &= ~(0xff << 8);
	rTCFG0 |= 3 << 8;			//prescaler = 3+1
	rTCFG1 &= ~(0xf << 12);
	rTCFG1 |= 0 << 12;		//mux = 1/2

	rTCNTB3 = val;
	rTCMPB3 = val >> 1;		// 50%
	rTCON &= ~(0xf << 16);
	rTCON |= 0xb << 16;		//interval, inv-off, update TCNTB3&TCMPB3, start timer 3
	rTCON &= ~(2 << 16);		//clear manual update bit
	while(time_ms--) {
		while(rTCNTO3 >= (val >> 1));
		while(rTCNTO3 < (val >> 1));
	}
}



